clear;
close all;
dbstop if error
%% 添加casadi
addpath("E:\Coding\matlab\casadi-3.6.7-windows64-matlab2018b");
import casadi.*
addpath("base_func\");
%% 处理线路数据
root_path = "real_line\";
folder_name = "北京北-崇礼\";
file_type = ".xls";

% GradientFilePath = strcat(root_path, folder_name, "坡道文件输入-递增", file_type);
% CurveFilePath = strcat(root_path, folder_name, "曲线文件输入-递增", file_type);
% SpeedLimitPath = strcat(root_path, folder_name, "限速文件输入-递增", file_type);
% GradientData = readmatrix(GradientFilePath);
% CurveData = readmatrix(CurveFilePath);
% SpeedLimitData = readmatrix(SpeedLimitPath);

% 线路2
GradientData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","坡道");
CurveData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","曲线");
SpeedLimitData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","限速");
CurveData = [CurveData(:,1:2).*1000 CurveData(:,4)];

%处理成仅3列，且把 km->m
GradientData = [GradientData(:,1:2).*1000 GradientData(:,3)];
CurveData = [CurveData(:,1:2).*1000 CurveData(:,3)];
SpeedLimitData = [SpeedLimitData(:,1:2).*1000 SpeedLimitData(:,3)];

%曲线数据补充平直道
CurveData = fill_curvedata(CurveData);
%补充缺失的限速数据
SpeedLimitData = fill_speedlimit(SpeedLimitData,250);

%% 结构体1：线路参数
LineInfo.gradient = GradientData;
LineInfo.curve = CurveData;
LineInfo.speedlimit = SpeedLimitData;
LineInfo.init_height = 640; %初始海拔

%% 结构体2：列车参数
TrainInfo.abc = [0.55 0.003622 0.00011]; %CRH380B
TrainInfo.mass = 425; %吨 CRH380B - AW1
TrainInfo.rf = 0.06; % rotate factor 回转质量系数
TrainInfo.eta_gear = 0.9; %齿轮箱输出和输入功率之比
TrainInfo.motor_num = 16;

%% 结构体3：求解参数
QsInfo.pos_start = 140;   %起点
QsInfo.pos_end = 260; %终点
QsInfo.v_start = 0; % km/h
QsInfo.v_end = 0;
QsInfo.set_time = 2800; %计划剩余时间
QsInfo.t_now = 0; %起始时刻
QsInfo.step_x = 5; %求解步长1
QsInfo.step_t = 5; 
QsInfo.t_err = 5; %达时运行浮动时间
QsInfo.T_st = [80 80]; %起始温度

%% 计算
%计算最大能力曲线
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% t_mp_min = state(end,3);
%计算恒速曲线
state = speedplan_cruise(LineInfo, TrainInfo, QsInfo);
tc_end = state(end,3);
%位置离散的恒速优化矩阵
xvtfhg = [state(:,1:3) state(:,4)-state(:,5) state(:,13) state(:,11)];
%转换为按时间离散的矩阵
td_ = 0:QsInfo.step_t:floor(tc_end)+QsInfo.t_err;
x_new = interp1(xvtfhg(:,3), xvtfhg(:,1), td_, 'linear');
v_new = interp1(xvtfhg(:,3), xvtfhg(:,2), td_, 'linear');
f_new = interp1(xvtfhg(:,3), xvtfhg(:,4), td_, 'linear');
h_new = interp1(xvtfhg(:,3), xvtfhg(:,5), td_, 'linear');
grd_new = interp1(xvtfhg(:,3), xvtfhg(:,6), td_, 'linear');
tvxfhg = [td_' v_new' x_new' f_new' h_new' grd_new'];
for i = length(td_)-floor(QsInfo.t_err/QsInfo.step_t)+1:length(td_)
    tvxfhg(i,2:6) = [xvtfhg(end,2) xvtfhg(end,1) xvtfhg(end,4) xvtfhg(end,5) xvtfhg(end,6)];
end

tvxfhg_temp = tvxfhg;
%权重： 速度 | 位置 | 温度 | 电牵制力代价 | 空气制动代价
w1 = 1e-1;
w2 = 1e-2;
w3 = 1e2;
w4 = 1e-2;
w5 = 20;
w6 = 1e3;
weights = [w1 w2 w3 w4 w5 w6];
%casadi sqp solve
% res = atc_sqp_opt_v3(tvxfhg_temp, weights, LineInfo, TrainInfo, QsInfo);
[res, casadi_flag] = atc_opt_gurobi(tvxfhg_temp, weights, LineInfo, TrainInfo, QsInfo, []);
% res = [td_' res];
s_opt_km = res(:,3)./1000;
v_opt_kmh = res(:,2)*3.6;
Ts_opt = res(:,4);
Tr_opt = res(:,5);
% xvtf_fig([s_opt' v_opt'*3.6 td_' Fe_opt'-Fm_opt'],LineInfo,QsInfo);
% figure(3)
% plot(tvxfhg(:,1),Tr_opt);
%% 牵引计算+计算温度
Tcr_(1,:) = QsInfo.T_st;
vcr = zeros(length(state),1);
tcr = zeros(length(state),1);
fcr_ = zeros(length(state),1);
mode = 0;
%位置离散牵引计算
for i = 1:length(state)-1
    % 牵引力
    fcr_(i) = state(i,4) - state(i,5);

    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,vcr(i)) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(state(i,1),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    a = (fcr_(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    vcr(i+1) = sqrt(vcr(i)*vcr(i)/3.6^2 + 2*a*QsInfo.step_x) * 3.6;
    % 更新过程时间
    dt = 2*QsInfo.step_x/((vcr(i+1) + vcr(i))/3.6);
    tcr(i+1) = tcr(i) + dt;

    v_kmh = (state(i,2)+state(i+1,2))/2;
    % v_kmh = state(i,2);
    if fcr_(i) < -getMaxFeb(v_kmh)
        Fpmt = -getMaxFeb(v_kmh)/TrainInfo.motor_num;
    else
        Fpmt = fcr_(i)/TrainInfo.motor_num;
    end

    [ploss(i,1),ploss(i,2),ploss(i,3),ploss(i,4)] = getmotorlossv2(Fpmt, v_kmh);
    dt = tcr(i+1)-tcr(i);
    [Tcr_(i+1,1), Tcr_(i+1,2)] = lptn2nodes_sr(ploss(i,1:2),Tcr_(i,1:2),state(i,13),dt);
end

%时间离散牵引计算
dt = 1;
ot = 0:dt:QsInfo.set_time+QsInfo.t_err; %离散时间序列
oT_(1,:) = QsInfo.T_st;
ox = zeros(length(ot),1);
ox(1) = QsInfo.pos_start * 1000;
ov_ms = zeros(length(ot),1);

of_ = zeros(length(ot),1);

for i = 1:length(ot)
    t = ot(i);
    for j = 1:length(res)-1
        if t >= res(j,1) && t <= res(j+1,1)
            of_(i) = res(j,6) - res(j,7);
            Fpmt = res(j,6) / TrainInfo.motor_num; %单个电机的发挥力
            break;
        end
    end
    % 牵引计算
    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,ov_ms(i)*3.6) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(ox(i),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    ta = (of_(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    ov_ms(i+1) = ov_ms(i) + ta*dt;
    % 计算下一状态位置
    ox(i+1) = ox(i) + ov_ms(i)*dt + 1/2*ta*dt^2;
    % 计算下一状态时间
    tn = t + dt;
    % 计算电机损耗
    [oploss(i,1),oploss(i,2),oploss(i,3),oploss(i,4)] = getmotorlossv2(Fpmt, ov_ms(i+1)*3.6);
    % 解算电机实际温度
    h = getPosHeight(ox(i+1),LineInfo);
    [oT_(i+1,1), oT_(i+1,2)] = lptn2nodes_sr(oploss(i,1:2),oT_(i,1:2),h,dt);
    % 判断制动停车
    if ov_ms(i+1) < 0
        ov_ms(i+1) = 0;
    end
end

%% 画图 位置轴
%值采样
% x_km = state(:,1)./1000;
vc_kmh = state(:,2);
% t_s = state(:,3);
% f_kN = f_;
% T_stator = real(T_(:,1));
% T_rotor = real(T_(:,2));
x_km = ox./1000;
v_kmh = ov_ms*3.6;
t_s = ot;
f_kN = [0; of_];
T_stator = real(oT_(:,1));
T_rotor = real(oT_(:,2));
fprintf("终点：%f km\n误差：%f m\n",x_km(end),QsInfo.pos_end*1000-ox(end));

%计算坡度
init_h = LineInfo.init_height;
h_(1) = init_h;
for j = 1:length(LineInfo.gradient)
    h_(j+1) = h_(j) + (LineInfo.gradient(j,2)-LineInfo.gradient(j,1))*LineInfo.gradient(j,3)/1000;
end
h_ = h_';
%处理限速
start_points = LineInfo.speedlimit(:, 1);
end_points = LineInfo.speedlimit(:, 2);
speed_values = LineInfo.speedlimit(:, 3);
vl_x = [];
vl_y = [];
for i = 1:size(LineInfo.speedlimit, 1)
    % 每段限速数据
    vl_x = [vl_x; start_points(i); end_points(i)];
    vl_y = [vl_y; speed_values(i); speed_values(i)];
end
vl_x = vl_x./1000; %m->km
%判断上下行
if QsInfo.pos_start > QsInfo.pos_end
    upordown = -1;
else
    upordown = 1;
end
figure(1)
set(gcf,'Position',[100,50,800,1000]);
[ha,pos] = tight_subplot(4, 1,[.005 .05],[.05 .03],[.08 .07]); % 缩小子图间距
axes(ha(1));
h15 = plot(state(:,1)./1000,vc_kmh,'LineWidth',1.5,'LineStyle','-','Color',[0.8500 0.3250 0.0980]);
set(h15, 'DisplayName', "恒速规划速度");
hold on;
hl2 = plot(x_km,v_kmh,'LineWidth',2,'Color',[0 0.4470 0.7410]);
set(hl2, 'DisplayName', "实际速度")

h13 = plot(s_opt_km,v_opt_kmh,'LineWidth',1.5,'Color',[0 0.4470 0.7410],'LineStyle','--');
set(h13, 'DisplayName', "casadi计算速度");
% hl3 = plot(new_mat(:,2),new_mat(:,8),'LineStyle',':','LineWidth',2.5,'Color',[0.8500 0.3250 0.0980]);
% set(hl3, 'DisplayName', "参考速度")
hl4 = plot(vl_x,vl_y,'LineStyle',':','LineWidth',1.5,'Color',	[1 0 1]);
set(hl4, 'DisplayName', "限速")
% xlim([min(new_mat(:,2))-0.2 max(new_mat(:,2))+0.2]);
% ylim([0 max(v_kmh)+20]);
xlabel("位置（km）");
ylabel("速度（km/h）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
lgd = legend('show','Location','south');
% axis([min(x_km)-0.2 max(x_km)+0.2 0 max(LineInfo.speedlimit(:,3))+20]);
axis([QsInfo.pos_start-0.2 QsInfo.pos_end+0.2 0 max(LineInfo.speedlimit(:,3))+20]);
% set(gca, 'XTickMode', 'auto', 'XTickLabelMode', 'auto');
% grid on;

% set(lgd, 'Position', [0, 0, 50, 60]); % 自定义位置
% title("空间域跟踪效果图");
axes(ha(2));
hl1 = area([LineInfo.gradient(:,1);LineInfo.gradient(end,2)]./1000,h_,'FaceColor',[0.7 0.7 0.7],'EdgeColor', [0.6 0.6 0.6],"FaceAlpha",0.35);
set(hl1, 'DisplayName', '坡度');
grid on;
legend("坡度")
% xlim([min(x_km)-0.2 max(x_km)+0.2]);
xlim([QsInfo.pos_start-0.2 QsInfo.pos_end+0.2]);
ylim([min(h_)-50 max(h_)*1.3]);
ylabel("相对海拔（m）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end

axes(ha(3));
plot(x_km,f_kN,'LineWidth',2);
hold on;
% plot(new_mat(:,2),new_mat(:,5),'LineStyle',':','LineWidth',2.5);
% plot(new_mat(:,2),new_mat(:,7),'LineStyle','--','LineWidth',1.5);
plot([-1 1e6],[0 0],'LineStyle','--','Color',[0 0 0])
xlabel("位置（km）");
ylabel("力（kN）");
% axis([min(x_km)-0.2 max(x_km)+0.2 min(f_kN)-50 max(f_kN)+50]);
axis([QsInfo.pos_start-0.2 QsInfo.pos_end+0.2 min(f_kN)-50 max(f_kN)+50]);
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
% grid on;
legend("实际力");

axes(ha(4))
plot(x_km,T_stator,'LineWidth',2);
hold on;
plot(x_km,T_rotor,'LineWidth',2);
plot(s_opt_km,Ts_opt,'LineWidth',1.5,'LineStyle','--');
plot(s_opt_km,Tr_opt,'LineWidth',1.5,'LineStyle','--');
grid on;
% axis([min(x_km)-0.2 max(x_km)+0.2 min(T_rotor)-20 max(T_rotor)+20]);
axis([QsInfo.pos_start-0.2 QsInfo.pos_end+0.2 min(T_rotor)-20 max(T_rotor)+20]);
xlabel("位置（km）");
ylabel("温度（℃）");
legend("定子温度","转子温度","casadi定子","casadi转子");

%% 画温度
% figure(5)
% plot(x_km,T_stator,'LineWidth',2);
% hold on;
% plot(x_km,T_rotor,'LineWidth',2);
% plot(s_opt/1000,Ts_opt,'LineStyle','--');
% plot(s_opt/1000,Tr_opt,'LineStyle','--');
% grid on;
% axis([min(x_km)-0.2 max(x_km)+0.2 min(T_rotor)-20 max(T_rotor)+20]);
% xlabel("位置（km）");
% ylabel("温度（℃）");
% legend("定子温度","转子温度","nlp定子","nlp转子");
% title()

%% 画图 时间轴
%恒速优化结果
ct_t = tvxfhg_temp(:,1);
ct_v_kmh = tvxfhg_temp(:,2);
ct_x_km = tvxfhg_temp(:,3)./1000;
%sqp优化
ot_t = res(:,1);
ot_v_kmh = res(:,2).*3.6;
ot_x_km = res(:,3)./1000;
%牵引计算结果
qt_t = [ot ot(end)+QsInfo.step_t];
qt_v_kmh = ov_ms.*3.6;
qt_x_km = ox./1000;

figure(7)
set(gcf,'Position',[100,50,1200,800]);
subplot(2,1,1);
plot(ct_t,ct_v_kmh,'LineWidth',1.5,'LineStyle','-','Color',[0.8500 0.3250 0.0980]);
hold on;
plot(ot_t,ot_v_kmh,'LineWidth',1.5,'Color',[0 0.4470 0.7410],'LineStyle','--');
plot(qt_t,qt_v_kmh,'LineWidth',2,'Color',[0 0.4470 0.7410]);
xlabel("时间（s）");
ylabel("速度（km/h）");
legend('恒速','sqp优化','实际','Location','south');
title('t-v图');

subplot(2,1,2);
plot(ct_t,ct_x_km,'LineWidth',1.5,'LineStyle','-','Color',[0.8500 0.3250 0.0980]);
hold on;
plot(ot_t,ot_x_km,'LineWidth',1.5,'Color',[0 0.4470 0.7410],'LineStyle','--');
plot(qt_t,qt_x_km,'LineWidth',2,'Color',[0 0.4470 0.7410]);
xlabel("时间（s）");
ylabel("位置（km）");
legend('恒速','sqp优化','实际','Location','south');
title('t-x图');

